#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <experimental/filesystem>
#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>

namespace fs = std::experimental::filesystem;
using namespace std;
using namespace cv;
// using namespace message_filters;

class syncNode
{
public:
  syncNode(string root_dir)
  {
    count_ = 0;
    path_save = root_dir+"/"; //must add "/" at last
    sub_1_.subscribe(nh_, "/senyun/front_left", 1);
    sub_2_.subscribe(nh_, "/senyun/front_mid", 1);
    sub_3_.subscribe(nh_, "/senyun/front_right", 1);
    sub_4_.subscribe(nh_, "/senyun/rear_left", 1);
    sub_5_.subscribe(nh_, "/senyun/rear_right", 1);
    // sub_6_.subscribe(nh_, "/senyun/top_left", 1);
    // sub_7_.subscribe(nh_, "/senyun/top_right", 1);
    sub_8_.subscribe(nh_, "/lidar/left", 1);
    sub_9_.subscribe(nh_, "/lidar/right", 1);
    sync_.reset(new Sync(MySyncPolicy(10), sub_1_, sub_2_, sub_3_, sub_4_, sub_5_,sub_8_,sub_9_));
    sync_->registerCallback(boost::bind(&syncNode::callback, this, _1, _2, _3, _4, _5,_6,_7));
  }
  void savePointCloudAsBin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, const std::string& filename)
  {
      // Open binary file for writing
      std::ofstream outfile(filename.c_str(), std::ios::out | std::ios::binary);
      // Write point cloud data to binary file
      for (size_t i = 0; i < cloud->points.size(); ++i)
      {
          outfile.write((const char*) &cloud->points[i].x, sizeof(float));
          outfile.write((const char*) &cloud->points[i].y, sizeof(float));
          outfile.write((const char*) &cloud->points[i].z, sizeof(float));
          outfile.write((const char*) &cloud->points[i].intensity, sizeof(float));
      }
      // Close binary file
      outfile.close();
  }

  void callback(const sensor_msgs::ImageConstPtr &front_left_msg,const sensor_msgs::ImageConstPtr &front_mid_msg,const sensor_msgs::ImageConstPtr &front_right_msg,
                const sensor_msgs::ImageConstPtr &rear_left_msg, const sensor_msgs::ImageConstPtr &rear_right_msg,
                const sensor_msgs::PointCloud2ConstPtr &pc_left_msg,const sensor_msgs::PointCloud2ConstPtr &pc_right_msg
                )
  {
    // if ((count_++ % 8) != 0)
    // {
    //   return;
    // }
    ROS_INFO("Synchronization successful");
    timestamp_ = ros::Time::now().toSec();
    string new_path_save = path_save + to_string(timestamp_);
    cv::Mat image = cv::Mat::zeros(front_mid_msg->height, front_mid_msg->width, CV_8UC3);
    // front left
    memcpy(image.data, front_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    cout << "save_path: " << new_path_save + "_front_left.png" << endl;
    bool ret = cv::imwrite(new_path_save + "_front_left.png", image);
    imshow("test", image);
    waitKey(1);
    cout << "saved: " << ret << endl;
    memcpy(image.data, front_mid_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    cv::imwrite(new_path_save + "_front_mid.png", image);
    memcpy(image.data, front_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    cv::imwrite(new_path_save + "_front_right.png", image);
    memcpy(image.data, rear_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    cv::imwrite(new_path_save + "_rear_left.png", image);
    memcpy(image.data, rear_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    cv::imwrite(new_path_save + "_rear_right.png", image);
    // memcpy(image.data, top_left_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    // cv::imwrite(new_path_save + "_top_left.png", image);
    // memcpy(image.data, top_right_msg->data.data(), sizeof(uint8_t) * image.rows * image.cols * 3);
    // cv::imwrite(new_path_save + "_top_right.png", image);

    pcl::PointCloud<pcl::PointXYZI>::Ptr p_l(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr p_r(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PCLPointCloud2 pc2_l,pc2_r;
    pcl_conversions::toPCL(*pc_left_msg, pc2_l);
    pcl_conversions::toPCL(*pc_right_msg, pc2_r);
    pcl::fromPCLPointCloud2(pc2_l, *p_l);
    pcl::fromPCLPointCloud2(pc2_r, *p_r);
    savePointCloudAsBin(p_l,new_path_save + "_left_lidar.bin");
    savePointCloudAsBin(p_r,new_path_save + "_right_lidar.bin");

  }

private:
  ros::NodeHandle nh_;
  message_filters::Subscriber<sensor_msgs::Image> sub_1_;
  message_filters::Subscriber<sensor_msgs::Image> sub_2_;
  message_filters::Subscriber<sensor_msgs::Image> sub_3_;
  message_filters::Subscriber<sensor_msgs::Image> sub_4_;
  message_filters::Subscriber<sensor_msgs::Image> sub_5_;
  // message_filters::Subscriber<sensor_msgs::Image> sub_6_;
  // message_filters::Subscriber<sensor_msgs::Image> sub_7_;
  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_8_;
  message_filters::Subscriber<sensor_msgs::PointCloud2> sub_9_;
  std::string path_save; // img dir
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::Image,
                                                          sensor_msgs::PointCloud2,sensor_msgs::PointCloud2> MySyncPolicy;
                                                          // sensor_msgs::Image,sensor_msgs::Image,
  typedef message_filters::Synchronizer<MySyncPolicy> Sync;
  boost::shared_ptr<Sync> sync_;
  double timestamp_;
  int count_;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "synchronizer");
  string root_dir = argv[1];
  //"/cv/DataSet/MVS/all_record_raw/q23_seq0/img5_lidar2"
  syncNode synchronizer(root_dir);

  ros::spin();
}
